#ifndef __GRAPH_SEARCH_ASTAR_H_INCLUDE__
#define __GRAPH_SEARCH_ASTAR_H_INCLUDE__

#include <vector>
#include <list>
#include "IndexedPriorityQLow.h"
#include "Heuristic_Euclid.h"
#include "GraphNode.h"

//Class implementation of an A* search
//This class will operate on a simple GrpahNode
//(in which case the search will perform more along the lines of
//a Dijkstra as no meaningful heuristic is employed) or a NavGraph
//which uses real cost vector distances to computer the H factor in the F = G + H. 
template <class graph_type, class heuristic>
class Graph_SearchAStar
{
private:
	typedef typename graph_type::EdgeType Edge;

private:
	const graph_type& m_Graph;

	//Indexed by node. Real cumulative cost to move to node
	std::vector<double> m_GCosts;
	std::vector<double> m_FCosts;

	std::vector<const Edge*> m_ShortestPathTree;
	std::vector<const Edge*> m_SearchFrontier;

	//Start and goal nodes
	int m_iSource;
	int m_iTarget;

	void Search();

public:
	Graph_SearchAStar(graph_type& graph, int source, int target)
		:m_Graph(graph),
		m_ShortestPathTree(graph.getNodeCount()),
		m_SearchFrontier(graph.getNodeCount()),
		m_GCosts(graph.getNodeCount(), 0.0),
		m_FCosts(graph.getNodeCount(), 0.0),
		m_iSource(source),
		m_iTarget(target)
	{
		Search();
	}

	std::vector<const Edge*> GetSPT()const{return m_ShortestPathTree;}
	double GetCostToTarget()const{return m_GCosts[m_iTarget];}

	std::list<int> GetPathToTarget()const;
};


//The search operation begins by iterating throught the nodes in the graph 
//computing the distance costs by way of the Euclid distance between the current node
//adn the goal.  The priority queue (or open set) stores the lower cost nodes at the top of
//the queue.  After each pass the node with the lowest total cost (previous cost + estimated cost)
//is removed and added to the solution set, and the adjusted costs (F) of each of the nodes 
//neighbors (located via the Edges) are updated
template <class graph_type, class heuristic>
void Graph_SearchAStar<graph_type, heuristic>::Search()
{
	IndexedPriorityQLow<double> pq(m_FCosts, m_Graph.getNodeCount());

	pq.insert(m_iSource);

	while(!pq.empty()){

		int NextClosestNode = pq.Pop();

		//Added node to the solution set
		m_ShortestPathTree[NextClosestNode] = m_SearchFrontier[NextClosestNode];

		//Success clause
		if(NextClosestNode == m_iTarget)
			return;

		graph_type::ConstEdgeIterator ConstEdgeItr(m_Graph, NextClosestNode);

		//Update neighbor nodes with adjusted cost
		for(const Edge* pE = ConstEdgeItr.begin(); !ConstEdgeItr.end(); pE=ConstEdgeItr.next())
		{
			double HCost = heuristic::Calculate(m_Graph, m_iTarget, pE->getDestination());
			double GCost = m_GCosts[NextClosestNode] + pE->getCost();

			if(m_SearchFrontier[pE->getDestination()] == NULL)
			{
				m_FCosts[pE->getDestination()] = GCost + HCost;
				m_GCosts[pE->getDestination()] = GCost;

				pq.insert(pE->getDestination());

				m_SearchFrontier[pE->getDestination()] = pE;
			}
			else if((GCost < m_GCosts[pE->getDestination()]) &&
				(m_ShortestPathTree[pE->getDestination()]==NULL))
			{
				m_FCosts[pE->getDestination()] = GCost + HCost;
				m_GCosts[pE->getDestination()] = GCost;

				pq.ChangePriority(pE->getDestination());

				m_SearchFrontier[pE->getDestination()] = pE;
			}
		}
	}
}

template <class graph_type, class heuristic>
std::list<int> Graph_SearchAStar<graph_type, heuristic>::GetPathToTarget()const
{
  std::list<int> path;

  //just return an empty path if no target or no path found
  if (m_iTarget < 0)  return path;    

  int nd = m_iTarget;

  path.push_front(nd);
    
  while ((nd != m_iSource) && (m_ShortestPathTree[nd] != 0))
  {
    nd = m_ShortestPathTree[nd]->getSource();

    path.push_front(nd);
  }

  return path;
} 

#endif